#ifndef IO_OBSTA_AVOID_H_
#define IO_OBSTA_AVOID_H_

#include <mutex>
#include <vector>
#include <string>
#include <unordered_map>
#include <utility>

#include "rclcpp/rclcpp.hpp"
#include "agv_msgs/msg/avoid_obsta.hpp"
#include "agv_msgs/msg/avoid_obsta_info.hpp"
#include "agv_msgs/msg/read_in_puts_new.hpp"
#include "agv_msgs/msg/pair_type_int.hpp"
#include "agv_msgs/msg/agv_auto_status.hpp"
#include "agv_msgs/msg/periph_status.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "agv_srvs/srv/outputs_new.hpp"
#include "tinyxml2.h"
#include "scenes_judge.hpp"

/*
    针对IO型避障激光雷达，规定各激光雷达io输入信号名称如下：
    左避障 leftStop   leftSlow   leftWarning
    右避障 rightStop  rightSlow  rightWarning
    前避障 frontStop  frontSlow  frontWarning
    后避障 rearStop   rearSlow   rearWarning
*/

namespace AgvObstaAvoid {

class IoObstaAvoid
{
private:
    /* data */
    rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr inputmap_sub_;
    rclcpp::Subscription<agv_msgs::msg::AgvAutoStatus>::SharedPtr auto_status_sub_;
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointstate_sub_;
    rclcpp::Subscription<agv_msgs::msg::PeriphStatus>::SharedPtr peripheral_sub_;
    rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr io_client_;
    bool lidar_error_;
    bool load_status_;

    struct fork_tip
    {
        bool lift_high = false;
        bool vehicle_type = false;
        bool vehicle_back = false;
        bool tip_sensor = false;
        bool load_inplace = false;
        bool task_id = false;
        bool action_type = false;
    } fork_avoid_;
    

    std::mutex io_State_mutex_;

    enum lidar_pose
    {
      FRONT,
      REAR,
      LEFT,
      RIGHT,
      LEFTFRONT,
      LEFTREAR,
      RIGHTFRONT,
      ROGHTREAR,
      TIPSENSOR,
      LOADINPLACE,
    };

    struct read_input
    {
      bool leftTipSensor;
      bool rightTipSensor;
      bool leftTipTouchSensor;
      bool rightTipTouchSensor;
      bool leftLoadInplace;
      bool rightLoadInplace;
      bool loadStatus;     
    } read_input_data_;

    struct InputSignal
    {
        std::vector<std::pair<std::string, bool>> input_list = {
            {"Stop", 0},           //0
            {"Slow", 0},           //1
            {"Warning", 0},        //2
            {"Error", 0},          //3
        };
    };
    
    InputSignal left_input_, right_input_, rear_input_, front_input_, 
                left_front_, left_rear_, right_front_, right_rear_, fork_tipsensor_, fork_loadinplace_;


    struct OssdSignal
    {
        std::vector<std::pair<std::string, bool>> ossd_input = {
            {"Ossd1", 0},           //0
            {"Ossd2", 0},           //1
        };
    };

    OssdSignal left_ossd_, right_ossd_, rear_ossd_;

    std::vector<std::string> leftzone_list_ = {
        {"leftZoneSwitch1"},       //0
        {"leftZoneSwitch2"},       //1
        {"leftZoneSwitch3"},       //2
        {"leftZoneSwitch4"},       //3
    };

    std::vector<std::string> leftFront_list_ = {
        {"leftFrontSwitch1"},       //0
        {"leftFrontSwitch2"},       //1
        {"leftFrontSwitch3"},       //2
        {"leftFrontSwitch4"},       //3
    };

    std::vector<std::string> leftRear_list_ = {
        {"leftRearSwitch1"},       //0
        {"leftRearSwitch2"},       //1
        {"leftRearSwitch3"},       //2
        {"leftRearSwitch4"},       //3
    };

    std::vector<std::string> rightzone_list_ = {
        {"rightZoneSwitch1"},      //0
        {"rightZoneSwitch2"},      //1
        {"rightZoneSwitch3"},      //2
        {"rightZoneSwitch4"},      //3
    };

    std::vector<std::string> rightFront_list_ = {
        {"rightFrontSwitch1"},      //0
        {"rightFrontSwitch2"},      //1
        {"rightFrontSwitch3"},      //2
        {"rightFrontSwitch4"},      //3
    };

    std::vector<std::string> rightRear_list_ = {
        {"rightRearSwitch1"},      //0
        {"rightRearSwitch2"},      //1
        {"rightRearSwitch3"},      //2
        {"rightRearSwitch4"},      //3
    };


    std::vector<std::string> rearzone_list_ = {
        {"rearZoneSwitch1"},       //0
        {"rearZoneSwitch2"},       //1
        {"rearZoneSwitch3"},       //2
        {"rearZoneSwitch4"},       //3
    };

    std::vector<std::string> frontzone_list_ = {
        {"frontZoneSwitch1"},      //0
        {"frontZoneSwitch2"},      //1
        {"frontZoneSwitch3"},      //2
        {"frontZoneSwitch4"},      //3
    };

    std::vector<std::string> io_output_list_;
    bool ioExist(std::string key);
    InputSignal posInput(int position);
    std::vector<std::string> getZoneSwitch(int position);

public:

    struct bArea
    {
        int name;
        std::vector<bool> interface;
    };
    
    struct bEquipment
    {
        int name;
        int docount;
        // 正前-0  正后-1  正左-2  正右-3  左前-4  左后-5  右前-6  右后-7  叉尖光电撞板-8  撞板及载货光电-9
        int pose;
        std::vector<bArea> arealist;
    };
    
    std::vector<bEquipment> io_equip_list_;
    std::unordered_map<int, std::vector<int>> output_interface_;
    std::shared_ptr<rclcpp::Node> node_;

    explicit IoObstaAvoid(rclcpp::Node::SharedPtr node);
    ~IoObstaAvoid() = default;

    bool IOReadXml(tinyxml2::XMLElement* element);
    void ShowEquipParam(bArea* area);
    void readInputCallBack(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg);
    void autoStatusCallBack(const agv_msgs::msg::AgvAutoStatus::SharedPtr msg);
    void jointStateCallBack(const sensor_msgs::msg::JointState::SharedPtr msg);
    void peripheralCallback(const agv_msgs::msg::PeriphStatus::SharedPtr load_msg);
    void refresh_avoidState(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg);
    std::vector<agv_msgs::msg::AvoidObsta> ScanCheck(std::vector<ScenesJudge::cEquip>& io_equip);
    bool outputMap2IO(std::vector<ScenesJudge::cEquip>& states);
    bool init();
    bool FaultDetec();
};

}   // namespace AgvObstaAvoid end

#endif